// svm training for Object identifier

#include <vector>

#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/ml/ml.hpp>

#include "ObjectIdentifier.h"

using namespace std;
using namespace cv;

namespace marker_detector
{

void ObjectIdentifier::training_from_pngdata()
{
  // parameter for SVM
  CvSVMParams params;
  params.svm_type = CvSVM::C_SVC;
  params.kernel_type = CvSVM::LINEAR;
  params.term_crit = cvTermCriteria(CV_TERMCRIT_ITER, 100, 1e-6);

  // training data
#if 1
  int datasetsize = 12;
  char _labelname[12][10] = {
    "apple", "mug", "teddy", "glasses", "dryer", "laptop",
    "glass", "hammer", "banana", "book", "scissors", "camera"
  };
#else
  int datasetsize = 3;
  char _labelname[12][10] = {
    "apple", "laptop", "glass"
  };
#endif
  for(int i = 0; i < datasetsize; i++) {
    label_.push_back(string(_labelname[i]));
  }

  // read datafile
  vector<Mat> _tmp(datasetsize);
  vector<Mat> tmp(datasetsize);
  for(int i = 0; i < datasetsize; i++) 
  {
    char filename[32];
    sprintf(filename, "robo_vtag2/vtag%02d_%s.png", i, label_[i].c_str());
    _tmp[i] = imread(filename, 0);
    assert(_tmp[i].data);
    tmp[i].create(MARKER_SIZE, MARKER_SIZE, CV_8UC1);
    resize(_tmp[i], tmp[i], tmp[i].size());
    threshold(tmp[i], tmp[i], 100, 255, THRESH_BINARY);
  }

  // create training data
  int size = MARKER_SIZE*MARKER_SIZE;
  float _t[datasetsize][size];
  float _l[datasetsize];
  for(int i = 0; i < datasetsize; i++) {
    _l[i] = (float)i;
    for(int j = 0; j < size; j++) {
      _t[i][j] = (float)tmp[i].data[j];
    }
  }
  Mat traindata_mat(datasetsize, size, CV_32FC1, _t);
  Mat labels_mat(datasetsize, 1, CV_32FC1, _l);

  // training
  svm_.train(traindata_mat, labels_mat, Mat(), Mat(), params);

}

void ObjectIdentifier::training_from_rosbag()
{
}


} // end of namespace
